#include "deke_motor.h"
#include "IT_Callback.h"
#include "string.h"
#include "myconfig.h"

// static PIDTypeDef DEKE_pid[4];
static PIDInstance DEKE_Pid_Instance[4];
static uint8_t deke_motor_number;
DEKE_TypeDef* DEKEDataHead_Handle = NULL;
static int DEKE_DataAttach(DEKE_TypeDef *this);

/**
 * @brief 初始化得科电机
 * 
 * @param this 电机自身得结构体
 * @param configs 定时器配置结构体
 * @param motor_mode 电机模式
 * @param wheel_position 电机位置,判断是左电机还是右电机
 * @param direction 电机转动方向
 * @param encoder_one_round 一圈得编码器值
 */
void DEKE_Init(DEKE_TypeDef* this,DEKE_CONFIG_T* configs,enum MotorMode motor_mode,Position wheel_position,enum MotorDirection direction,uint16_t encoder_one_round)
{
    memset(this,0,sizeof(DEKE_TypeDef));
    this->config = configs;
    // this->pid_handle = &DEKE_pid[deke_motor_number++];
    // PID_Init(this->pid_handle, 0, 6000, 0.08, 0.5, 0);
    this->pid_handle = &DEKE_Pid_Instance[deke_motor_number++];
    PID_Init_Config_s config ={
        .Kp = 8.0f,
        .Ki = 32.0f,
        .Kd = 0.0f,
        .MaxOut = 3000.0f, //限幅
        .DeadBand =5,//差值大于5才进行PID调节，防止抖动
        .Improve = PID_DeltaT_Limit,
        .DeltaT_Limit_Max = 0.1f, //时间间隔限幅
        .DeltaT_Limit_Min = 0.0001f, //时间间隔限幅
        //... 剩下的是用于优化的可选参数
    };
    PIDInit(this->pid_handle, &config);
    this->motor_mode = motor_mode;
    this->wheel_position = wheel_position;
    this->encoder_one_round = encoder_one_round;
    this->direction = direction;
    this->next = NULL;
    DEKE_DataAttach(this);


    HAL_TIM_PWM_Start(configs->PWM_handle,configs->tim_channel_1);
    HAL_TIM_PWM_Start(configs->PWM_handle,configs->tim_channel_2);
    if (configs->encoder_handle != NULL)
        HAL_TIM_Encoder_Start(configs->encoder_handle, TIM_CHANNEL_ALL);
}

/**
 * @brief 设置电机速度
 * 
 * @param this 电机自己
 * @param speed 速度大小
 */
void DEKE_SetSpeed(DEKE_TypeDef* this, int16_t speed)
{
    this->speed = speed;
}

/**
 * @brief 设置真实速度
 * 
 * @param this 电机
 * @param PWM 要设置得PWM值，范围为(-8000 - 8000)
 */
void DEKE_SetRealSpeed(DEKE_TypeDef* this,int16_t PWM)
{
    TIM_HandleTypeDef* PWM_TIM = this->config->PWM_handle;
    uint32_t tim_channel_1 = this->config->tim_channel_1;
    uint32_t tim_channel_2 = this->config->tim_channel_2;
    // 限幅
    PWM = limit(PWM, -8000, 8000);

    // 更新电机结构体的PWM值
    this->PWM = PWM;

    // 电机速度补偿
    PWM = PWM * SPEED_COMPENSATION;
    PWM = limit(PWM, -8000, 8000);

    // 正转
    if (this->direction == MOTOR_NOMRAL) {
        if(PWM >= 0) {
            __HAL_TIM_SetCompare(PWM_TIM, tim_channel_2, 0);
            __HAL_TIM_SetCompare(PWM_TIM, tim_channel_1, PWM);
        } else {
            __HAL_TIM_SetCompare(PWM_TIM, tim_channel_1, 0);
            __HAL_TIM_SetCompare(PWM_TIM, tim_channel_2, -PWM);
        }
    }
    // 反转
    else {
        if(PWM >= 0) {
            __HAL_TIM_SetCompare(PWM_TIM, tim_channel_1, 0);
            __HAL_TIM_SetCompare(PWM_TIM, tim_channel_2, PWM);
        } else {
            __HAL_TIM_SetCompare(PWM_TIM, tim_channel_2, 0);
            __HAL_TIM_SetCompare(PWM_TIM, tim_channel_1, -PWM);
        }
    }
    
}

/**
 * @brief 获得当前得编码器值
 * 
 * @param this 电机自己
 * @return int16_t 返回的编码器值
 */
int16_t DEKE_GetMotorPWM(DEKE_TypeDef* this)
{
    return this->PWM;
}

/**
 * @brief 比较函数，用于qsort排序
 */
static int compareMotorSpeedPWMInfo(const void *a, const void *b) {
    struct MotorSpeedPWMInfo *infoA = (struct MotorSpeedPWMInfo *)a;
    struct MotorSpeedPWMInfo *infoB = (struct MotorSpeedPWMInfo *)b;
    return (infoA->speed - infoB->speed);
}

/**
 * @brief 通过速度获取电机PWM值
 * 
 * @param motorSpeedPWMInfo 电机的一些速度与PWM值的对应关系
 * @param size motorSpeedPWMInfo数组的大小
 * @param speed 速度，单位为每0.1s的编码器脉冲数
 * @return int16_t 
 */
int16_t getMotorPWMBySpeed(struct MotorSpeedPWMInfo motorSpeedPWMInfo[], uint8_t size,int16_t speed) {
      // 先对数组进行排序,如果数组已经排好序，可以不用排序
    //qsort(motorSpeedPWMInfo, size, sizeof(struct MotorSpeedPWMInfo), compareMotorSpeedPWMInfo);
      if (motorSpeedPWMInfo == NULL) {
        return 0;
      }
      if (size == 0) {
        return 0;
      }
      if (size == 1) {
        return motorSpeedPWMInfo[0].PWM;
      }

      // 速度为负数，先转为正数
      int sign = 1;
      if (speed < 0) {
        speed = -speed;
        sign = -1;
      }
      
      // 如果速度小于最小速度，在最小速度的基础上使用线性插值，通过最小的两个点算出
      if (speed <= motorSpeedPWMInfo[0].speed) {
        int16_t speedDiff = motorSpeedPWMInfo[1].speed - motorSpeedPWMInfo[0].speed;
        int16_t pwmDiff = motorSpeedPWMInfo[1].PWM - motorSpeedPWMInfo[0].PWM;
        return sign *
               (motorSpeedPWMInfo[0].PWM +
                (speed - motorSpeedPWMInfo[0].speed) * pwmDiff / speedDiff);
      }

      // 如果速度大于最大速度，在最大速度的基础上使用线性插值，通过最大的两个点算出
      if (speed >= motorSpeedPWMInfo[size - 1].speed) {
        int16_t speedDiff = motorSpeedPWMInfo[size - 1].speed -
                            motorSpeedPWMInfo[size - 2].speed;
        int16_t pwmDiff = motorSpeedPWMInfo[size - 1].PWM - motorSpeedPWMInfo[size - 2].PWM;
        return sign * (motorSpeedPWMInfo[size - 2].PWM + (speed - motorSpeedPWMInfo[size - 2].speed) 
                                                          * pwmDiff / speedDiff);
      }

    // 通过线性插值找到对应的PWM值
    for (uint8_t i = 0; i < size - 1; i++) {
        if (speed >= motorSpeedPWMInfo[i].speed && speed <= motorSpeedPWMInfo[i + 1].speed) {
            int16_t speedDiff = motorSpeedPWMInfo[i + 1].speed - motorSpeedPWMInfo[i].speed;
            int16_t pwmDiff = motorSpeedPWMInfo[i + 1].PWM - motorSpeedPWMInfo[i].PWM;
            return sign*(motorSpeedPWMInfo[i].PWM + (speed - motorSpeedPWMInfo[i].speed) * pwmDiff / speedDiff);
        }
    }

    // 如果没有找到合适的区间，返回0（理论上不会到达这里）
    return 0;
}
float getMotorSpeedByPWM(struct MotorSpeedPWMInfo motorSpeedPWMInfo[], uint8_t size,int16_t PWM) {
    // 先对数组进行排序,如果数组已经排好序，可以不用排序
    //qsort(motorSpeedPWMInfo, size, sizeof(struct MotorSpeedPWMInfo), compareMotorSpeedPWMInfo);

    if (motorSpeedPWMInfo == NULL) {
        return 0;
    }
    if (size == 0) {
        return 0;
    }
    if (size == 1) {
        return motorSpeedPWMInfo[0].speed;
    }

    // 如果PWM为负数，先转为正数
    int sign = 1;
    if(PWM < 0){
        PWM = -PWM;
        sign = -1;
    }
    // 如果PWM小于最小PWM，在最小PWM的基础上使用线性插值
    if (PWM <= motorSpeedPWMInfo[0].PWM) {
        int16_t speedDiff = motorSpeedPWMInfo[1].speed - motorSpeedPWMInfo[0].speed;
        int16_t pwmDiff = motorSpeedPWMInfo[1].PWM - motorSpeedPWMInfo[0].PWM;
        return sign*(motorSpeedPWMInfo[0].speed + (PWM - motorSpeedPWMInfo[0].PWM) * speedDiff / pwmDiff);
    }

    // 如果PWM大于最大PWM，在最大PWM的基础上使用线性插值
    if (PWM >= motorSpeedPWMInfo[size - 1].PWM) {
        int16_t speedDiff = motorSpeedPWMInfo[size - 1].speed - motorSpeedPWMInfo[size - 2].speed;
        int16_t pwmDiff = motorSpeedPWMInfo[size - 1].PWM - motorSpeedPWMInfo[size - 2].PWM;
        return sign*(motorSpeedPWMInfo[size - 2].speed + (PWM - motorSpeedPWMInfo[size - 2].PWM) * speedDiff / pwmDiff);
    }

    // 通过线性插值找到对应的速度
    for (uint8_t i = 0; i < size - 1; i++) {
        if (PWM >= motorSpeedPWMInfo[i].PWM && PWM <= motorSpeedPWMInfo[i + 1].PWM) {
            int16_t speedDiff = motorSpeedPWMInfo[i + 1].speed - motorSpeedPWMInfo[i].speed;
            int16_t pwmDiff = motorSpeedPWMInfo[i + 1].PWM - motorSpeedPWMInfo[i].PWM;
            return sign*(motorSpeedPWMInfo[i].speed + (PWM - motorSpeedPWMInfo[i].PWM) * speedDiff / pwmDiff);
        }
    }

    // 如果没有找到合适的区间，返回0（理论上不会到达这里）
    return 0;
}

/**
 * @brief PID速度环控制，需要轮询
 * 
 */
void DEKE_SpeedAdjustment_PID(void)
{
    DEKE_TypeDef* this = DEKEDataHead_Handle;
    int setPWM = 0;
    while (this)
    {
        if (this->reverseFlag == 1) {
            DEKE_MotorStop(this);
            this = this->next;
            // 电机插反了，自己检查一下
            continue;
        }
        if (this->lastFlag == 0) {
            this->lastFlag = 1;
            this->lastError = this->encoder - this->speed;
        }
        // 计算当前误差
        int16_t currentError = this->encoder - this->speed;
        // 电机插反检测
        if (abs16(currentError) > abs16(this->lastError) + 10) {
            this->errorNum++;
            if (this->errorNum >= 5) {
                this->reverseFlag = 1;
                DEKE_MotorStop(this);
                this = this->next;
                // 电机插反了
                continue;
            }
        } else {
            this->errorNum = 0;
        }
        this->lastError = currentError;
        // 编码器计数器和设定速度的差值大于5才进行PID调节，防止抖动
        // if (abs16(currentError) > 5) {
        //     setPWM = positionPIDCalc_LimitSumError(this->pid_handle,this->encoder);
        // }
        setPWM = PIDCalculate(this->pid_handle,this->encoder,this->speed);
        DEKE_SetRealSpeed(this,setPWM);

        this = this->next;
    }
    
}

/**
 * @brief 改变电机模式
 * 
 * @param this 电机自己
 * @param motor_mode 要设置模式
 *      @arg MOTOR_MODE_Direct 直接操纵PWM
 *      @arg MOTOR_MODE_SPEED 通过速度环控制速度
 */
void DEKE_ChangeMode(DEKE_TypeDef *this,enum MotorMode motor_mode)
{
    this->motor_mode = motor_mode;
}

/**
 * @brief 停止电机
 * 
 * @param this 需要停止的电机
 */
void DEKE_MotorStop(DEKE_TypeDef *this)
{
    DEKE_ChangeMode(this,MOTOR_MODE_Direct);
    DEKE_SetRealSpeed(this,0);
    this->speed = 0;
}

/**
 * @brief 获得当前电机的模式
 * 
 * @param this 电机自己
 * @return enum MotorMode 
 */
enum MotorMode DEKE_getMotorRunMode(DEKE_TypeDef *this) {
    return this->motor_mode;
}

/**
 * @brief 电机的编码器值，需要轮询
 * 
 * @return uint8_t 是否获取成功，1为成功，2为失败
 */
uint8_t DEKE_Encoder_Task(void)
{
    static uint8_t callFlag = 0;         // 用于判断是否是第一次调用
    static uint8_t retval = 0;
    static uint32_t lastTime_hight = 0;  // 全局时间的值
    static uint32_t lastTime_low = 0;    // timer6的CNT值

    uint32_t currentTime_hight = getGlobalTime();
    uint32_t currentTime_low = __HAL_TIM_GET_COUNTER(&TIM_HANDLE_GLOBAL_TIME);

    // 如果是第一次调用或者超过1秒没有调用,则需要再重新进入一次获取下一次的时间间隔
    if (callFlag == 0 || currentTime_hight - lastTime_hight > 1000) {  
        callFlag = 1;                   // 标记为已调用
        retval = 0;
    } else {
        retval = 1;
        DEKE_TypeDef* this = DEKEDataHead_Handle;
        uint32_t interval_hight = currentTime_hight - lastTime_hight;
        uint32_t interval_sum = interval_hight * 10 + currentTime_low - lastTime_low;  // 单位1/10000秒
        while (this)
        {
            TIM_HandleTypeDef* encoder_handle = this->config->encoder_handle;
            if (encoder_handle != NULL)
            {
                // 统一一圈的脉冲数为572，要先统计一圈有几个脉冲，然后再更改宏定义，这里做了一个统一为一圈572的脉冲数的转换
                int16_t encoderNum =
                    (float)(int16_t)__HAL_TIM_GET_COUNTER(encoder_handle) /
                    this->encoder_one_round * ENCODER_ONE_STANDARD;
                this->encoder = ((double)encoderNum) / interval_sum * 1000;
                // 如果是右电机，则需要取反
                if (this->wheel_position == Right_wheel) {
                    this->encoder = -this->encoder;
                }

                // 把计数器置0
                __HAL_TIM_SET_COUNTER(encoder_handle, 0);
            }
            else
            {
                this->encoder = DEKE_Get_encoder(this->config->other_motor_encoder);
            }
            this = this->next;
        }
    }
    // 更新lastTime
    lastTime_hight = currentTime_hight;
    lastTime_low = currentTime_low;
    return retval;
}

/**
 * @brief 获得当前的编码器值
 * 
 * @param this 电机自己
 * @return int16_t 返回的编码器值
 */
int16_t DEKE_Get_encoder(DEKE_TypeDef *this)
{
    return this->encoder;
}

/**
 * @brief 将该结构体添加到链表中
 * @param  this 指向自己的指针
 * @return -1表示已经注册，0表示成功注册
 */
static int DEKE_DataAttach(DEKE_TypeDef *this)
{
	DEKE_TypeDef* target = DEKEDataHead_Handle;
	while(target) {
		if(target == this) return -1;	//表示已经注册
		target = target->next;
	}
	this->next = DEKEDataHead_Handle;
	DEKEDataHead_Handle = this;
	return 0;
}

